#ifndef __FRUSTUMMATH_H__
#define __FRUSTUMMATH_H__

#include "Plane.h"
#include <cassert>

#define ANG2RAD(x) (acos(-1)/180.0)*(x)

class FrustumMath{
 public:
	double ratio;
	double FOV;
	Vector3 ntl,ntr,nbl,nbr,ftl,ftr,fbl,fbr;
	double nh,nw,fh,fw,tang;
	double nearD;
	double farD;

	Plane pl[6];

 FrustumMath() : ratio(1.0), FOV(60.0), ntl(0) ,ntr(0),nbl(0),nbr(0),ftl(0),ftr(0),fbl(0),fbr(0),nh(0),nw(0),fh(0),fw(0),tang(0),nearD(0),farD(0) { }
	~FrustumMath() { }

	void setFrustumSpecs(float FOV, float ratio, float nearD, float farD) {
		this->FOV = FOV;
		this->ratio = ratio;
		this->nearD = nearD;
		this->farD = farD;

		// compute heights and widths based on FOV
		tang = (float)tan(ANG2RAD(FOV) * 0.5) ;
		nh = nearD * tang;
		nw = nh * ratio; 
		fh = farD  * tang;
		fw = fh * ratio;
	}

	void setCamSpecs(Vector3 p, Vector3 l, Vector3 u) {
		Vector3 dir,nc,fc,X,Y,Z;

		Z = p - l;
		Z.normalize();

		X = u * Z;
		X.normalize();

		Y = Z * X;

		nc = p - Z * nearD;
		fc = p - Z * farD;

		ntl = nc + Y * nh - X * nw;
		ntr = nc + Y * nh + X * nw;
		nbl = nc - Y * nh - X * nw;
		nbr = nc - Y * nh + X * nw;

		ftl = fc + Y * fh - X * fw;
		ftr = fc + Y * fh + X * fw;
		fbl = fc - Y * fh - X * fw;
		fbr = fc - Y * fh + X * fw;

		pl[TOP].initialize_from_points(ntr,ntl,ftl);

		pl[BOTTOM].initialize_from_points(nbl,nbr,fbr);
		pl[LEFT].initialize_from_points(ntl,nbl,fbl);
		pl[RIGHT].initialize_from_points(nbr,ntr,fbr);
		pl[NEARP].initialize_from_points(ntl,ntr,nbr);
		pl[FARP].initialize_from_points(ftr,ftl,fbl);
	}

	bool point_inside(Vector3 p) {
		for(int i=0; i < 6; i++) {
			if (pl[i].distance(p) < 0)
				return false;
		}

		return true;
	}

	double min_distance(Vector3 p) { // negative if outside!
		double dist=pl[0].distance(p);

		if (isnan(dist)) {
			assert(false);
		}

		
		for (int i=1; i < 6; i++) {
			dist = min(dist, pl[i].distance(p));
			assert(!isnan(dist));
		}

		return dist;
	}

	void print() {
		for (int i=0; i<6; i++) {
			pl[i].print();
		}
	}

 private:
	enum {
		TOP = 0,
		BOTTOM,
		LEFT,
		RIGHT,
		NEARP,
		FARP
	};

};


#endif
